#include "cybertron/sensor/noise/GPSNoiseMaker.hpp"
#include "cybertron/sensor/noise/LinearTimeNoise.hpp"
#include "cybertron/sensor/noise/PeriodicTimeNoise.hpp"
#include "cybertron/sensor/noise/PinkNoise.hpp"

CYBERTRON_BEGIN

GPSNoiseMaker::GPSNoiseMaker(float horizontalError/* = 1.0*/, float verticalError/* = 0.6*/, float velocityError/* = 0.02*/, float attitudeError/* = 0.0026 in rad*/)
	: m_horizontalError(horizontalError),
	m_verticalError(verticalError),
	m_velocityError(velocityError),
	m_attitudeError(attitudeError),
	m_velocityNoise(0.0, velocityError), 
	m_attitudeNoise(0.0, attitudeError)
{
	if (m_horizontalError > 0 || m_verticalError > 0) {
		m_positionNoise.addNoise(std::make_shared<PeriodicTimeNoise>(m_horizontalError, m_verticalError));
		m_positionNoise.addNoise(std::make_shared<LinearTimeNoise>());
		m_positionNoise.addNoise(std::make_shared<PinkNoise>(3600.f, m_horizontalError, m_verticalError, 0.0f));
		m_positionNoise.addNoise(std::make_shared<WhiteNoise>(0.0f, m_horizontalError * 0.05f, 0.0f, m_verticalError * 0.05f));
	}
}

GPSNoiseMaker::GPSNoiseMaker(const SensorGpsImuConfig& sensorConfig)
	: GPSNoiseMaker(sensorConfig.HorizontalPositionError, sensorConfig.VerticalPositionError, sensorConfig.VelocityError, sensorConfig.AttitudeError)
{
	m_sensorConfig = sensorConfig;
}
 
GPSNoiseMaker::~GPSNoiseMaker()
{
}

void GPSNoiseMaker::update(const vec3& pos, const vec3& rotation, uint64_t frame/* = 0*/)
{
	m_positionNoise.update();
	m_velocityNoise.update();
	m_attitudeNoise.update();
	NoiseMaker::update(pos, rotation);
}

CYBERTRON_END